/*
 * @Description: 后端任务管理文件
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:06:00
 * @LastEditTime: 2021-10-27 11:07:23
 * @LastEditors: Sang Hao
 */
#ifndef LIDAR_SLAM_MAPPING_BACK_END_BACK_END_FLOW_HPP_
#define LIDAR_SLAM_MAPPING_BACK_END_BACK_END_FLOW_HPP_

#include <ros/ros.h>

#include "lidar_slam/subscriber/cloud_subscriber.hpp"
#include "lidar_slam/subscriber/odometry_subscriber.hpp"
#include "lidar_slam/subscriber/loop_pose_subscriber.hpp"

#include "lidar_slam/publisher/odometry_publisher.hpp"
#include "lidar_slam/publisher/key_frames_publisher.hpp"
#include "lidar_slam/publisher/key_frame_publisher.hpp"

#include "lidar_slam/mapping/back_end/back_end.hpp"

namespace lidar_slam {
class BackEndFlow {
private:
	/* data */
	std::shared_ptr<CloudSubscriber> cloud_sub_ptr_;
	std::shared_ptr<OdometrySubscriber> laser_odom_sub_ptr_;
	std::shared_ptr<LoopPoseSubscriber> loop_pose_sub_ptr_;

	std::shared_ptr<OdometryPublisher> transformed_odom_pub_ptr_;
	std::shared_ptr<KeyFramePublisher> key_frame_pub_ptr_;
	std::shared_ptr<KeyFramesPublisher> key_frames_pub_ptr_;
	std::shared_ptr<BackEnd> back_end_ptr_;

	std::deque<CloudData> cloud_data_buff_;
	std::deque<PoseData> laser_odom_data_buff_;
	std::deque<LoopPose> loop_pose_data_buff_;

	PoseData current_laser_odom_data_;
	CloudData current_cloud_data_;

private:
	bool ReadData();
	bool MaybeInsertLoopPose();
	bool HasData();
	bool ValidData();
	bool UpdateBackEnd();
	bool PublishData();
	
public:
	BackEndFlow(ros::NodeHandle& nh);
	bool Run();
	bool ForceOptimize();
};
}


#endif